﻿#include <gtest/gtest.h>

#include <learn/math/matrix.h>
#include <learn/math/matrix_solver.h>
#include <learn/utils/adaptor_eigen.h>
#include <third/dbg.h>

#include <Dense>

#include <ctime>
#include <random>

using namespace xi;
using namespace xi::math;

class TestMatrix : public ::testing::Test
{
  protected:
    void SetUp() override
    {
    }

    void TearDown() override
    {
        // nothing
    }

    MatrixXd Init(int m, int n, std::initializer_list<double> lst)
    {
        MatrixXd A(m, n);
        double *p = A.data();
        std::copy(lst.begin(), lst.end(), p);
        return A;
    }
};

// 测试 MatrixXd
TEST_F(TestMatrix, Initialize)
{
    // 测试比较
    {
        EXPECT_TRUE(Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                    Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));
        EXPECT_TRUE(Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                    Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));
        EXPECT_FALSE(Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) == Init(3, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9}));
    }

    // 测试初始化
    {
        auto A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_TRUE((A.rows() == 3 && A.cols() == 4));

        auto A2 = MatrixXd(4, 3).set_value(1.1);
        EXPECT_TRUE(A2 == Init(4, 3, {1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1}));

        MatrixXd A3 = A2;
        EXPECT_TRUE(A3 == A2);

        MatrixXd A4 = MatrixXd::identity(5);
        MatrixXd A5(5, 5);
        A5.set_zero();
        for (int i = 0; i < 5; i++)
            A5(i, i) = 1;
        EXPECT_TRUE(A4 == A5);
    }

    // 测试赋值
    {
        auto A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        auto A2 = MatrixXd(3, 4).set_zero();
        A2 = A;
        EXPECT_TRUE(A2 == A);

        // 自赋值
        A2 = A2;
        EXPECT_TRUE(A2 == A);

        auto A3 = MatrixXd(3, 4).set_value(1.1);
        A3 = std::move(A2);
        EXPECT_TRUE(A3 == A);

        // 自移动
        A3 = std::move(A3);
    }

    // 辅助
    {
        MatrixXd A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        MatrixXd B = Init(2, 3, {1, 2, 3, 5, 6, 7});
        A.resize(2, 3, true);
        EXPECT_TRUE(A == B);
    }

    // 测试 ()
    {
        double arr[] = {1.2, 2.5, 34, 4.8, 5.1, 6.9, 7.1, 8.2, 9.7, 10.5, 11.4, 12.2};

        // 测试取引用
        auto A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        for (int i = 0; i < A.rows(); i++)
            for (int j = 0; j < A.cols(); j++)
                A(i, j) = arr[i * 4 + j];
        for (int i = 0; i < A.rows(); i++)
            for (int j = 0; j < A.cols(); j++)
                EXPECT_DOUBLE_EQ(A(i, j), arr[i * 4 + j]);

        EXPECT_EQ(A.rows(), 3);
        EXPECT_EQ(A.cols(), 4);
    }
}

// 测试 MatrixXd
TEST_F(TestMatrix, OpFunctions)
{
    // 测试四则运算
    {
        auto A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        MatrixXd A2 = A * 2;
        EXPECT_TRUE(A2 == Init(3, 4, {2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24}));

        MatrixXd A3 = A2 / 2;
        EXPECT_TRUE(A3 == A);

        MatrixXd A4 = A + A;
        EXPECT_TRUE(A4 == A2);

        MatrixXd A5 = A2 - A;
        EXPECT_TRUE(A5 == A);

        MatrixXd B = Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        MatrixXd A6 = A * B;
        EXPECT_TRUE(A6 == Init(3, 3, {70, 80, 90, 158, 184, 210, 246, 288, 330}));

        MatrixXd A7 = -A;
        EXPECT_TRUE((A7 + A) == MatrixXd(3, 4).set_zero());
    }

    // 测试自运算
    {
        auto A = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        A *= 2;
        EXPECT_TRUE(A == Init(3, 4, {2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24}));

        A /= 2;
        EXPECT_TRUE(A == Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));

        A += A;
        EXPECT_TRUE(A == Init(3, 4, {2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24}));

        A -= A;
        EXPECT_TRUE(A == Init(3, 4, {0, 0, 0, 0, 0}));
    }

    // Kronecker 积
    {
        auto A = Init(3, 1, {1, 2, 3});
        auto B = Init(2, 3, {1, 2, 3, 4, 5, 6});
        auto C = Init(6, 3, {1, 2, 3, 4, 5, 6, 2, 4, 6, 8, 10, 12, 3, 6, 9, 12, 15, 18});
        EXPECT_TRUE((A % B) == C);
    }

    // 填充矩阵
    {
        auto A = MatrixXd(3, 4).set_value(1.1);
        auto B = MatrixXd(2, 3).set_value(2.2);
        A.fill(1, 1, B);
        EXPECT_TRUE(A == Init(3, 4, {1.1, 1.1, 1.1, 1.1, 1.1, 2.2, 2.2, 2.2, 1.1, 2.2, 2.2, 2.2}));
    }

    // 范数
    {
        auto A = Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_EQ(A.norm(0), 33);
        EXPECT_DOUBLE_EQ(std::sqrt(A.norm(2)), 25.495097567963924);
    }

    // 测试迹
    {
        auto A = Init(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        EXPECT_DOUBLE_EQ(A.trace(), 16);

        auto B = Init(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_DOUBLE_EQ(B.trace(), 18);

        auto C = Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        EXPECT_DOUBLE_EQ(C.trace(), 15);
    }

    // 取元素
    {
        auto A = Init(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});

        EXPECT_TRUE(A.transposed() == Init(3, 4, {1, 4, 7, 10, 2, 5, 8, 11, 3, 6, 9, 12}));
        EXPECT_TRUE(A.transpose() == Init(3, 4, {1, 4, 7, 10, 2, 5, 8, 11, 3, 6, 9, 12}));
    }

    // 求逆
    {
        MatrixXd A = Init(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        MatrixXd B = A.inverted();
        EXPECT_LE((B - Init(3, 3, {-2.0 / 3, -2.0 / 3, 1.0, -4.0 / 3, 11.0 / 3, -2.0, 1.0, -2.0, 1.0})).norm(), 1e-8);
        EXPECT_LE((B - A.invert()).norm(), 1e-8);

        MatrixXd A1 = Init(2, 2, {5, 6, 1, 3});
        MatrixXd B1 = A1.inverted();
        EXPECT_LE((B1 - Init(2, 2, {3, -6, -1, 5}) / 9).norm(), 1e-8);
        EXPECT_LE((B1 - A1.invert()).norm(), 1e-8);
    }

    // 测试行列式
    {
        for (int i = 0; i < 10; i++)
        {
            MatrixXd A(4, 4);
            A.set_random();

            Eigen::Matrix4d matrix;
            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    matrix(i, j) = A(i, j);

            double determinant = matrix.determinant(); // 计算行列式
            double det = A.deted();
            EXPECT_FLOAT_EQ(det, determinant);
            EXPECT_FLOAT_EQ(A.det(), determinant);
        }
    }

    // 测试矩阵秩
    {
        MatrixXd A(6, 9);
        A.set_random();

        Eigen::MatrixXd matrix(6, 9);
        for (int i = 0; i < 6; i++)
            for (int j = 0; j < 9; j++)
                matrix(i, j) = A(i, j);

        // 计算矩阵秩
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);
        double tolerance = 1e-10; // 判定奇异值为零的容差
        int rank = (svd.singularValues().array() > tolerance).count();
        int r = std::abs(A.ranked());
        EXPECT_EQ(r, rank);
        EXPECT_EQ(std::abs(A.rank()), rank);
    }

    {
        MatrixXd A(9, 6);
        A.set_random();

        Eigen::MatrixXd matrix(9, 6);
        for (int i = 0; i < 9; i++)
            for (int j = 0; j < 6; j++)
                matrix(i, j) = A(i, j);

        // 计算矩阵秩
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);
        double tolerance = 1e-10; // 判定奇异值为零的容差
        int rank = (svd.singularValues().array() > tolerance).count();
        int r = std::abs(A.ranked());
        EXPECT_EQ(r, rank);
        EXPECT_EQ(std::abs(A.rank()), rank);
    }

    // 计算模最大的特征值
    {
        MatrixXd A(9, 9);
        A.set_random();

        auto adp = utils::Adaptor_EigenMatrix(A);
        Eigen::MatrixXd matrix = adp;

        matrix.eigenvalues();
        double max_eigenvalue = 0;
        for (int i = 0; i < 9; i++)
        {
            double eigenvalue = matrix.eigenvalues()[i].real();
            if (std::abs(eigenvalue) > max_eigenvalue)
                max_eigenvalue = eigenvalue;
        }

        double eigenvalue = std::abs(A.eigenvalue());
        std::cout << "max eigenvalue: " << max_eigenvalue << std::endl;
        std::cout << "eigenvalue: " << eigenvalue << std::endl;
        // EXPECT_DOUBLE_EQ(eigenvalue, max_eigenvalue);
    }
}

TEST_F(TestMatrix, Vector)
{
    // 测试向量
    {
        Vector v(3);
        v[0] = 1;
        v[1] = 2;
        v[2] = 3;
        EXPECT_EQ(v.size(), 3);
        EXPECT_DOUBLE_EQ(v[0], 1);
        EXPECT_DOUBLE_EQ(v[1], 2);
        EXPECT_DOUBLE_EQ(v[2], 3);

        Vector w(3);
        w[0] = 4;
        w[1] = 5;
        w[2] = 6;
        Vector u = v + w;
        EXPECT_DOUBLE_EQ(u[0], 5);
        EXPECT_DOUBLE_EQ(u[1], 7);
        EXPECT_DOUBLE_EQ(u[2], 9);
    }

    // 测试拷贝构造、移动构造、赋值
    {
        Vector v(3);
        v[0] = 1;
        v[1] = 2;
        v[2] = 3;

        Vector w(v);
        EXPECT_DOUBLE_EQ(w[0], 1);
        EXPECT_DOUBLE_EQ(w[1], 2);
        EXPECT_DOUBLE_EQ(w[2], 3);

        Vector u(std::move(w));
        EXPECT_DOUBLE_EQ(u[0], 1);
        EXPECT_DOUBLE_EQ(u[1], 2);
        EXPECT_DOUBLE_EQ(u[2], 3);

        w = u;
        EXPECT_DOUBLE_EQ(w[0], 1);
        EXPECT_DOUBLE_EQ(w[1], 2);
        EXPECT_DOUBLE_EQ(w[2], 3);

        w = w;
        EXPECT_DOUBLE_EQ(w[0], 1);
        EXPECT_DOUBLE_EQ(w[1], 2);
        EXPECT_DOUBLE_EQ(w[2], 3);

        w = std::move(u);
        EXPECT_DOUBLE_EQ(w[0], 1);
        EXPECT_DOUBLE_EQ(w[1], 2);
        EXPECT_DOUBLE_EQ(w[2], 3);
    }

    // 测试矩阵构造
    {
        MatrixXd A(3, 1);
        A.set_random();

        Vector v = A;
        EXPECT_TRUE(MatrixXd(v) == A);
        EXPECT_EQ(v.size(), 3);

        MatrixXd B(1, 3);
        B.set_random();

        Vector w = B;
        EXPECT_TRUE(MatrixXd(w) == B);
        EXPECT_EQ(w.size(), 3);

        Vector v2 = std::move(A);
        EXPECT_TRUE(v == v2);

        Vector w2 = std::move(B);
        EXPECT_TRUE(w == w2);
    }

    // 测试向量点积
    {
        Vector v(3);
        v[0] = 1;
        v[1] = 2;
        v[2] = 3;

        Vector w(3);
        w[0] = 4;
        w[1] = 5;
        w[2] = 6;

        double dot_vw = dot(v, w);
        EXPECT_DOUBLE_EQ(dot_vw, 32);
    }

    // 测试矩阵与向量乘法
    {
        MatrixXd A(3, 3);
        A(0, 0) = 1;
        A(0, 1) = 2;
        A(0, 2) = 3;
        A(1, 0) = 4;
        A(1, 1) = 5;
        A(1, 2) = 6;
        A(2, 0) = 7;
        A(2, 1) = 8;
        A(2, 2) = 9;

        Vector b(3);
        b[0] = 1;
        b[1] = 2;
        b[2] = 3;

        Vector c = A * b;
        EXPECT_DOUBLE_EQ(c[0], 14);
        EXPECT_DOUBLE_EQ(c[1], 32);
        EXPECT_DOUBLE_EQ(c[2], 50);
    }
}

TEST_F(TestMatrix, SparseVec)
{
    {
        MatrixXd A(8, 4);
        A.set_random();

        SparseVec sv;
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                sv.create(i, j, A(i, j));

        double inner = 0;
        sv.foreach ([&](int i, int j, double value) { inner += value * value; });

        EXPECT_DOUBLE_EQ(inner, A.norm(2));
    }

    {
        // 测试张量积
        SparseVec<double> a, b;
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 4; j++)
            {
                a.create(i, j, i * 4 + j);
                b.create(j, i, i * 4 + j);
            }
        }

        MatrixXd A(12, 1), B(1, 12);

        auto A1 = a.to_matrix();
        auto B1 = b.to_matrix();

        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 4; j++)
            {
                A(i * 4 + j, 0) = A1(i, j);
            }
        }

        for (int i = 0; i < 4; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                B(0, i * 3 + j) = B1(i, j);
            }
        }

        std::cout << A << std::endl;
        std::cout << B << std::endl;

        auto c = a % b;
        auto C = A * B;
        MatrixXd C1(12, 12);
        C1.set_zero();

        auto f = [&](int x, int y, double value) { C1(x, y) = value; };
        c.foreach (f);

        EXPECT_EQ(C1, C);
    }
}

// 测试 SparseMat
TEST_F(TestMatrix, SparseMat)
{
    // 测试初始化
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));

        MatrixXd B(4, 8);
        B.set_zero();
        SparseMat SB(4, 8);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(j, i) = value;
            SB(j, i) = value;
        }

        EXPECT_EQ(SA.rows(), 8);
        EXPECT_EQ(SA.cols(), 4);

        SparseMat SB2(4, 8);
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                SB2(j, i) = 1;
        EXPECT_EQ(SB2.nnz(), 32);

        MatrixXd C = MatrixXd::identity(4);
        SparseXd SC = SparseXd::identity(4);
        EXPECT_TRUE(MatrixXd(SC) == C);
    }

    // 测试赋值 + 隐式转化
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        SparseMat SB = SA;
        EXPECT_TRUE(MatrixXd(SB) == MatrixXd(SA));

        SparseMat SC(8, 4);
        SC = SA;
        EXPECT_TRUE(MatrixXd(SC) == MatrixXd(SA));

        SB = SB;
        EXPECT_TRUE(MatrixXd(SB) == MatrixXd(SA));

        SparseMat SD = std::move(SC);
        EXPECT_TRUE(MatrixXd(SD) == MatrixXd(SA));

        SD = std::move(SD);
        EXPECT_TRUE(MatrixXd(SD) == MatrixXd(SA));
    }

    // 测试 resize, transpose
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        SparseMat SB = SA;
        SA.resize(4, 8);
        for (int i = 0; i < 4; i++)
        {
            for (int j = 0; j < 8; j++)
            {
                SA(i, j) = A(j, i);
            }
        }
        EXPECT_TRUE(MatrixXd(SA) == MatrixXd(SB.transposed()));
        EXPECT_TRUE(MatrixXd(SA) == MatrixXd(SB.transpose()));
    }

    // 测试加减
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        MatrixXd B(8, 4);
        B.set_zero();
        SparseMat SB(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(i, j) = value;
            SB(i, j) = value;
        }

        MatrixXd C = A + B;
        SparseMat SC = SA + SB;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));

        C = A - B;
        SC = SA - SB;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));

        C = -C;
        SC = -SC;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));
    }

    // 测试自加自减
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        A += A;
        SA += SA;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));

        A -= A;
        SA -= SA;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));
    }

    // 测试数乘除
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        MatrixXd C = A / 3;
        SparseMat SC = SA / 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));

        C = A * 3;
        SC = SA * 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));
    }

    // 测试自数乘除
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        A /= 3;
        SA /= 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));

        A *= 3;
        SA *= 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));
    }

    // 测试乘矩阵
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        MatrixXd B(4, 8);
        B.set_zero();
        SparseMat SB(4, 8);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(j, i) = value;
            SB(j, i) = value;
        }

        MatrixXd C = A * B;
        SparseMat SC = SA * SB;
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 8; j++)
                EXPECT_DOUBLE_EQ(C(i, j), SC(i, j));

        MatrixXd D = B * A;
        SparseMat SD = SB * SA;
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(D(i, j), SD(i, j));
    }
}

TEST_F(TestMatrix, SparseNumeric)
{
    // 测试范数
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        double normA = A.norm();
        double normSA = SA.norm();
        EXPECT_DOUBLE_EQ(normA, normSA);

        double normA2 = A.norm(2);
        double normSA2 = SA.norm(2);
        EXPECT_DOUBLE_EQ(normA2, normSA2);
    }

    // 测试 trace
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        double traceA = A.trace();
        double traceSA = SA.trace();
        EXPECT_DOUBLE_EQ(traceA, traceSA);
    }

    // 测试压缩
    {
        srand(time(0));

        MatrixXd A(8, 4);
        A.set_zero();
        SparseMat SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA(i, j) = value;
        }

        SparseMat SB = SA.compressed();
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SB(i, j));

        SA.compress();
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_DOUBLE_EQ(A(i, j), SA(i, j));
    }
}
TEST_F(TestMatrix, Tridiagonal)
{
    // 测试取元素
    {
        int N = 10;
        MatrixXd data(3, N);
        data.set_random();

        Tridiagonal m(N);
        MatrixXd A(N, N);
        A.set_zero();
        for (int i = 0; i < 10; i++)
        {
            for (int j = 0; j < 10; j++)
            {
                if (i == j)
                {
                    m(i, j) = A(i, j) = data(1, i);
                }
                else if (i == j + 1)
                {
                    m(i, j) = A(i, j) = data(2, i);
                }
                else if (i == j - 1)
                {
                    m(i, j) = A(i, j) = data(0, i);
                }
            }
        }

        for (int i = 0; i < 10; i++)
        {
            for (int j = 0; j < 10; j++)
            {
                if (std::abs(i - j) <= 1)
                    EXPECT_DOUBLE_EQ(m(i, j), A(i, j));
            }
        }
    }

    // 测试初始化、拷贝构造、移动构造、赋值
    {
        Tridiagonal m1(10);
        m1.set_random();

        MatrixXd A(10, 10);
        A.set_zero();

        for (int i = 0; i < 10; i++)
        {
            for (int j = std::max(0, i - 1); j < std::min(i + 2, 10); j++)
            {
                A(i, j) = m1(i, j);
            }
        }
        EXPECT_EQ(m1.to_matrix(), A);

        Tridiagonal m2 = m1;
        EXPECT_EQ(m2.to_matrix(), A);

        Tridiagonal m3(std::move(m2));
        EXPECT_EQ(m3.to_matrix(), A);

        MatrixXd B = m1;
        Tridiagonal m4(B);
        EXPECT_EQ(m4, m1);

        Tridiagonal m5 = std::move(B);
        EXPECT_EQ(m5, m1);

        MatrixXd C = m1.to_matrix();
        Tridiagonal m6 = Tridiagonal::from_matrix(C);
        EXPECT_EQ(m6.to_matrix(), A);

        Tridiagonal m7(10);
        m7 = m1;
        EXPECT_EQ(m7.to_matrix(), A);

        Tridiagonal m8(10);
        m8 = std::move(m1);
        EXPECT_EQ(m8.to_matrix(), A);
    }

    // 四则运算
    {
        Tridiagonal m1(10);
        m1.set_random();

        Tridiagonal m2(10);
        m2.set_random();

        MatrixXd A = m1.to_matrix();
        MatrixXd B = m2.to_matrix();

        Tridiagonal m3 = m1 + m2;
        MatrixXd C = A + B;
        EXPECT_EQ(m3.to_matrix(), C);

        Tridiagonal m4 = m1 - m2;
        MatrixXd D = A - B;
        EXPECT_EQ(m4.to_matrix(), D);

        auto m5 = m1 * m2;
        MatrixXd E = A * B;
        EXPECT_EQ(m5.to_matrix(), E);

        Tridiagonal m6 = m1 / 2;
        MatrixXd F = A / 2;
        EXPECT_EQ(m6.to_matrix(), F);

        Tridiagonal m7 = m1 * 2;
        MatrixXd G = A * 2;
        EXPECT_EQ(m7.to_matrix(), G);
    }

    // 自运算
    {
        Tridiagonal m1(10);
        m1.set_random();

        Tridiagonal m2(10);
        m2.set_random();

        MatrixXd A = m1.to_matrix();
        MatrixXd B = m2.to_matrix();

        m1 += m2;
        A += B;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 -= m2;
        A -= B;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 /= 2;
        A /= 2;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 *= 2;
        A *= 2;
        EXPECT_EQ(m1.to_matrix(), A);
    }

    // 操作函数
    {
        Tridiagonal m1(10);
        m1.set_random();

        MatrixXd A = m1.to_matrix();
        EXPECT_DOUBLE_EQ(A.norm(0), m1.norm(0));
        EXPECT_DOUBLE_EQ(A.norm(1), m1.norm(1));
        EXPECT_DOUBLE_EQ(A.norm(2), m1.norm(2));
        EXPECT_DOUBLE_EQ(A.trace(), m1.trace());

        Tridiagonal m2 = m1.transposed();
        MatrixXd B = A.transposed();

        EXPECT_EQ(B, m2.to_matrix());
        EXPECT_EQ(A.transpose(), m1.transpose().to_matrix());
    }
}

TEST_F(TestMatrix, Pentadiagonal)
{
    // 测试取元素
    {
        int N = 10;
        MatrixXd data(5, N);
        data.set_random();

        Pentadiagonal m(N);
        MatrixXd A(N, N);
        A.set_zero();
        for (int i = 0; i < 10; i++)
        {
            for (int j = 0; j < 10; j++)
            {
                if (i == j - 2)
                    m(i, j) = A(i, j) = data(0, i);
                else if (i == j - 1)
                    m(i, j) = A(i, j) = data(1, i);
                else if (i == j)
                    m(i, j) = A(i, j) = data(2, i);
                else if (i == j + 1)
                    m(i, j) = A(i, j) = data(3, i);
                else if (i == j + 2)
                    m(i, j) = A(i, j) = data(4, i);
            }
        }

        for (int i = 0; i < 10; i++)
        {
            for (int j = 0; j < 10; j++)
            {
                if (std::abs(i - j) <= 2)
                    EXPECT_DOUBLE_EQ(m(i, j), A(i, j));
            }
        }
    }

    // 测试初始化、拷贝构造、移动构造、赋值
    {
        Pentadiagonal m1(10);
        m1.set_random();

        MatrixXd A(10, 10);
        A.set_zero();

        for (int i = 0; i < 10; i++)
        {
            for (int j = std::max(0, i - 2); j < std::min(i + 3, 10); j++)
            {
                A(i, j) = m1(i, j);
            }
        }
        EXPECT_EQ(m1.to_matrix(), A);

        Pentadiagonal m2 = m1;
        EXPECT_EQ(m2.to_matrix(), A);

        Pentadiagonal m3(std::move(m2));
        EXPECT_EQ(m3.to_matrix(), A);

        MatrixXd B = m1;
        Pentadiagonal m4(B);
        EXPECT_EQ(m4, m1);

        Pentadiagonal m5 = std::move(B);
        EXPECT_EQ(m5, m1);

        MatrixXd C = m1.to_matrix();
        Pentadiagonal m6 = Pentadiagonal::from_matrix(C);
        EXPECT_EQ(m6.to_matrix(), A);

        Pentadiagonal m7(10);
        m7 = m1;
        EXPECT_EQ(m7.to_matrix(), A);

        Pentadiagonal m8(10);
        m8 = std::move(m1);
        EXPECT_EQ(m8.to_matrix(), A);
    }

    // 四则运算
    {
        Pentadiagonal m1(10);
        m1.set_random();

        Pentadiagonal m2(10);
        m2.set_random();

        MatrixXd A = m1.to_matrix();
        MatrixXd B = m2.to_matrix();

        Pentadiagonal m3 = m1 + m2;
        MatrixXd C = A + B;
        EXPECT_EQ(m3.to_matrix(), C);

        Pentadiagonal m4 = m1 - m2;
        MatrixXd D = A - B;
        EXPECT_EQ(m4.to_matrix(), D);

        auto m5 = m1 * m2;
        MatrixXd E = A * B;
        EXPECT_EQ(m5.to_matrix(), E);

        Pentadiagonal m6 = m1 / 2;
        MatrixXd F = A / 2;
        EXPECT_EQ(m6.to_matrix(), F);

        Pentadiagonal m7 = m1 * 2;
        MatrixXd G = A * 2;
        EXPECT_EQ(m7.to_matrix(), G);
    }

    // 自运算
    {
        Pentadiagonal m1(10);
        m1.set_random();

        Pentadiagonal m2(10);
        m2.set_random();

        MatrixXd A = m1.to_matrix();
        MatrixXd B = m2.to_matrix();

        m1 += m2;
        A += B;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 -= m2;
        A -= B;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 /= 2;
        A /= 2;
        EXPECT_EQ(m1.to_matrix(), A);

        m1 *= 2;
        A *= 2;
        EXPECT_EQ(m1.to_matrix(), A);
    }

    // 操作函数
    {
        Pentadiagonal m1(10);
        m1.set_random();

        MatrixXd A = m1.to_matrix();
        EXPECT_DOUBLE_EQ(A.norm(0), m1.norm(0));
        EXPECT_DOUBLE_EQ(A.norm(1), m1.norm(1));
        EXPECT_DOUBLE_EQ(A.norm(2), m1.norm(2));
        EXPECT_DOUBLE_EQ(A.trace(), m1.trace());

        Pentadiagonal m2 = m1.transposed();
        MatrixXd B = A.transposed();

        EXPECT_EQ(B, m2.to_matrix());
        EXPECT_EQ(A.transpose(), m1.transpose().to_matrix());
    }
}

TEST_F(TestMatrix, Inline)
{
    // 测试 dot
    {
        int N = 10;
        MatrixXd A(N, N);
        A.set_random();

        EXPECT_DOUBLE_EQ(dot(A, A), A.norm(2));

        double l = 10;
        double r = 20;
        EXPECT_DOUBLE_EQ(dot(l, r), l * r);
    }

    // 测试 divide
    {
        auto [A, x, b] = Matrix_LLT_Solver::example(1, 10);
        MatrixXd B(10, 10);
        B.set_random();

        EXPECT_EQ(left_divide(A, B), A.inverted() * B);

        double l = 10;
        double r = 20;
        EXPECT_DOUBLE_EQ(left_divide(l, r), r / l);
    }

    // 测试 householder
    {
        MatrixXd X(10, 1);
        X.set_random();
        auto Y = X;
        auto beta = householder(Y.data(), Y.rows());
        auto H = MatrixXd::identity(X.rows());
        H = H - Y * Y.transposed() * beta;
        Y = H * X; // 所有分量集中在第一个元素中
        EXPECT_DOUBLE_EQ(X.norm(2), std::pow(Y(0, 0), 2));
    }
}